/*
*  PresenceDetector.cpp
* 
*  Implementation of the Presence Detector
*
*  (c) 2011, Microsoft Applied Sciences Group
*
*  Author: Sven Kratz
*
*/

#include "stdafx.h"
#include "OpenCVDLL.h"
								


using namespace OpenCVDLL;

PresenceDetector::PresenceDetector(int width, int height)
{
    avg_ctr = 0;
    frame_count = 0;
	sensitivity_ = DEFAULT_SENSITIVITY;

    imgWidth = width;
    imgHeight = height;

    cascade = (CvHaarClassifierCascade*) cvLoad( FACE_CASCADE, 0,0,0);

    if (!cascade)
    {
        throw gcnew System::IO::FileLoadException("Cannot load Haar Cascade!");
    }

    storage = cvCreateMemStorage(0);


    imgSize = new CvSize; 

    imgSize->width = width; 
    imgSize->height = height; 

    // set up image buffers
    movingAverage = cvCreateImage(*imgSize, IPL_DEPTH_32F, 3);
    activityHeatmap = cvCreateImage(*imgSize, IPL_DEPTH_32F, 1);
    cvSet(activityHeatmap, cvScalar(0,0,0));
    diffGray = cvCreateImage(*imgSize, IPL_DEPTH_8U, 1);
    difference = cvCreateImage(*imgSize, 8, 3);


    // algorithm intialization
    first = true; 
    avg_ctr = 0;
    frame_count = 0;

	faces_hist = new std::vector<CvRect>*[AVG_FRAMES+1];
	for (int i = 0; i < AVG_FRAMES + 1; i++)
	{
		faces_hist[i] = new std::vector<CvRect>;
	}
	last_faces = new std::vector<CvRect>;


    // result
    presence = false; 

}

PresenceDetector::~PresenceDetector()
{
	// get explicit pointers to the images due to some compiler problem
	IplImage * mAvg = movingAverage; 
	IplImage * actH = activityHeatmap;
	IplImage * dG = diffGray; 
	IplImage * diff = difference; 
	CvHaarClassifierCascade * casc = cascade; 


	// do the releases
	cvReleaseImage(&mAvg); 
	cvReleaseImage(&actH); 
	cvReleaseImage(&dG);
	cvReleaseImage(&diff); 
	cvFree(&casc);


}

void PresenceDetector::ProcessFrame(unsigned char* imageData) 

{
	IplImage * img = cvCreateImage(*imgSize, 8,3); 
    img->imageData = (char*) imageData;

    // run the presence algorithm
    DetectMotion(img); 
    DetectFaceAndDecidePresence(img); 

	frame_count++;
	cvReleaseImage(&img); 

}

///
///	Simply returns the value of presence
///
bool PresenceDetector::GetCurrentPresence()
{
	return presence; 
}

///
/// Attempt at returning a managed list of points which contains the presence locations
///
cli::array<System::Drawing::Point^>^ PresenceDetector::GetCurrentPresenceLocations()
{
	if (!presence)
	{
		return nullptr; 
	}
	else
	{
		return  gcnew array<System::Drawing::Point^>(0); 
	}
}



////////////////////////////////////////////////////////////// Private /////////////////////////////////////////////////////////////////

///
///	Motion Detection Step
///
void PresenceDetector::DetectMotion(IplImage * image) 
{
    
    if (first)
        {	
            temp = cvCloneImage(image);
            //cvConvertScale(image, movingAverage, 1.0, 0.0);
            first = false;
            return;
        }

    CvSize imgsize = cvSize(image->width, image->height);

    IplImage * paintActivity = cvCreateImage(imgsize, IPL_DEPTH_8U, 1);
    cvSet(paintActivity, cvScalar(0,0,0));


    cvRunningAvg(image, movingAverage, sensitivity_, 0);
    

	//cvConvertScale(movingAverage, temp, 1.0, 0.0);
    cvAbsDiff(image, temp, difference);
    cvCvtColor(difference,diffGray, CV_RGB2GRAY);

    cvSmooth(diffGray, diffGray, CV_GAUSSIAN,3,3);
    cvThreshold(diffGray, diffGray, 70, 255, CV_THRESH_BINARY);
    cvDilate(diffGray, diffGray, 0, 18);
    cvErode(diffGray, diffGray, 0, 10);

    ////////////////////////////////
    IplImage * grayCtr = cvCloneImage(diffGray);
    ////////////////////////////////

    // contours
    ////////////////////////////////
    
	CvSeq * contour = contour_;
   

    cvFindContours( grayCtr, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
    
    if (contour == 0)
    {
        numContours = 0;
    }
    else
    {
        //int numPoints = contour->total * 4;

        CvRect bndRect = cvRect(0,0,0,0);
        int prevX;

        // contour array index
        int cIdx = 0;

        for( ; contour != 0; contour = contour->h_next )
        {
            CvPoint pt1;
            CvPoint pt2;

            //Get a bounding rectangle around the moving object.
            bndRect = cvBoundingRect(contour, 0);

            pt1.x = bndRect.x;
            pt1.y = bndRect.y;
            pt2.x = bndRect.x + bndRect.width;
            pt2.y = bndRect.y + bndRect.height;

            cvRectangle(paintActivity, pt1, pt2, CV_RGB(1,1,1), CV_FILLED, 8, 0);
        }

    }

    cvRunningAvg(paintActivity, activityHeatmap, sensitivity_,0);

    //cvShowImage("motion", temp) ;

    // "Garbage Collection"
    cvReleaseImage(&grayCtr);
    cvReleaseImage(&paintActivity);
    cvClearMemStorage(storage);
	
}


///
///	Face Detection + Decision Step
///
void PresenceDetector::DetectFaceAndDecidePresence(IplImage * image) 
{
    int scale = 1; 
	int i = 0;
    
    // clear previously used mem storage
    cvClearMemStorage(storage);

    if (cascade)
    {
        // There can be more than one face in an image. So create a growable sequence of faces.
        // Detect the objects and store them in the sequence
        CvSeq* faces = cvHaarDetectObjects( image, cascade, storage,
                                            1.1, 2, CV_HAAR_DO_CANNY_PRUNING,
                                            cvSize(40, 40) );

		// "garbage collection" --> a simple clear() doesn't cut it
		
		faces_hist[avg_ctr]->clear();

        // Loop the number of faces found.
        for( i = 0; i < (faces ? faces->total : 0); i++ )
        {
            
           // Create a new rectangle for drawing the face
            CvRect* r = (CvRect*)cvGetSeqElem( faces, i );

            /*
            CvRect rr; 
            rr.x = r->x;
            rr.y = r->y;
            rr.width = r->width;
            rr.height = r->height; */ 

            faces_hist[avg_ctr]->push_back(*r);


            // Find the dimensions of the face,and scale it if necessary
            /*
            pt1.x = r->x*scale;
            pt2.x = (r->x+r->width)*scale;
            pt1.y = r->y*scale;
            pt2.y = (r->y+r->height)*scale;

            // Draw the rectangle in the input image
            cvRectangle( img, pt1, pt2, CV_RGB(0,255,0), 3, 8, 0 ); */ 

        }

        printf("Detected %d faces this frame \n", faces_hist[avg_ctr]->size());
        
        // find correspondences in faces history

        //faces_averages.clear();

        
                        
        if (frame_count > AVG_FRAMES) 
        {
            if (last_faces->size() == 0)
            {
                for (int i = 0; i < faces_hist[avg_ctr]->size(); i++)
                {
                    last_faces->push_back((*faces_hist[avg_ctr])[i]);
                }
            }

            int last_faces_size = last_faces->size();

            // copy constructor
            std::vector<CvRect> faces(*last_faces); 

            // clear last faces
            last_faces->clear();

            for (int i = 0; i <last_faces_size; i++)
            {

                CvRect r = faces[i]; 

                std::vector<CvRect> correspondences;
                correspondences.push_back(r); 

                CvPoint c1,c2; 
                c1.x = r.x;
                c1.y = r.y; 
                c2.x = r.x + r.width;
                c2.y = r.y + r.height; 

                bool foundCorresp = false; 

                for (int j = 1; j< AVG_FRAMES; j++)
                {
                
                    foundCorresp = false; 
                    int prev_index = abs((avg_ctr-j) % AVG_FRAMES);
                    printf ("[j] %d prev_index %d", j, prev_index); 
                    for (int k = 0; k < faces_hist[prev_index]->size(); k++)
                    {
                        printf ("\t[k] %d\n", k); 
                        CvRect cr = (*faces_hist[prev_index])[k]; 
                        CvPoint corr1, corr2;
                        corr1.x = cr.x;
                        corr1.y = cr.y; 
                        corr2.x = cr.x + cr.width; 
                        corr2.y = cr.y + cr.height;

                        float d1 = sqrtf((c1.x-corr1.x)*(c1.x-corr1.x) + (c1.y-corr1.y)*(c1.y-corr1.y));
                        float d2 = sqrtf((c2.x-corr2.x)*(c2.x-corr2.x) + (c2.y-corr2.y)*(c2.y-corr2.y));

                        //printf("Distances: \t %f \t %f \n", d1, d2); 

                        // try if it is a correspondence
                        if ( sqrtf((c1.x-corr1.x)*(c1.x-corr1.x) + (c1.y-corr1.y)*(c1.y-corr1.y)) <=  RADIUS && 
                             sqrtf((c2.x-corr2.x)*(c2.x-corr2.x) + (c2.y-corr2.y)*(c2.y-corr2.y)) <= RADIUS)
                        {
                            foundCorresp  = true; 
                            correspondences.push_back(cr); 
                            //break;
                        }

                    }
                    if (foundCorresp = false)
                        break; 
                }

                if (correspondences.size() >1)
                {
                    //printf("Found %d Correspondences \n", correspondences.size()); 
                    // draw them

                    CvPoint avg1, avg2;
                    avg1.x = avg1.y = avg2.x = avg2.y = 0; 
                    int size = correspondences.size(); 
                    for (int n = 0; n < size; n++)
                    {
                        CvRect cr = correspondences[n]; 
                        avg1.x += cr.x;
                        avg1.y += cr.y; 

                        avg2.x += cr.x + cr.width;
                        avg2.y += cr.y + cr.height; 
                    }

                    avg1.x /= size; 
                    avg1.y /= size;
                    avg2.x /= size;
                    avg2.y /= size; 


                    // Draw the rectangle in the input image
                    // cvRectangle( img, avg1, avg2, CV_RGB(0,0,255), 3, 8, 0 );
                    // cvRectangle( motionFaceComb, avg1, avg2, CV_RGB(0,0,255), 3, 8, 0 );

                    CvRect face;
                    face.x = avg1.x;
                    face.y = avg1.y;
                    face.width = avg2.x - avg1.x;
                    face.height = avg2.y - avg1.y;
                    last_faces->push_back(face);
                    
                }

            }
        }

		// increment the ring buffer index
		avg_ctr = ++avg_ctr % AVG_FRAMES;

		// set the internal field to the presence decision 
		presence = Decide_Presence(*last_faces, activityHeatmap);

    }
}

bool PresenceDetector::Decide_Presence(std::vector<CvRect>& faces, IplImage * activity)
{
	printf("======== Average Activity of detected Face Region ========\n");	
	bool pr = false; 

	for (int i = 0; i < faces.size(); i++)
	{
		CvRect face = faces[i]; 

		if (face.width >= FACE_MIN_SIZE && face.height >= FACE_MIN_SIZE)
		{
			cvSetImageROI(activity, face); 
			CvScalar average = cvAvg(activity); 
			printf("Region\t%d\tAverage\t%f\n", i, average.val[0]); 

			if (average.val[0] > 0.05) 
			{
				pr = true; 
			}
		}

	}
	cvResetImageROI(activity);
	return pr;  
}





